/*
 * Copyright (C) 2012 Google Inc.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */

#ifndef GOOGLE_MAPPING_MAP_H_
#define GOOGLE_MAPPING_MAP_H_

#include <cmath>

#include <Eigen/Geometry>
#include <nav_msgs/OccupancyGrid.h>
#include <octomap/OcTree.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <tf/tf.h>

namespace google_mapping {

class Map {
 public:
  // Returns a new instance of Map with the specified resolution in meters per
  // cell.
  Map(float resolution);

  void insertPointCloud(
    const pcl::PointCloud<pcl::PointXYZ>& point_cloud,
    const tf::Pose& pose);

  double getResolution() const;
  double getProbability(double x, double y, int level) const;
  double getProbability(double x, double y, double z, int level) const;
  Eigen::Vector3f get2DInterpolatedProbabilityWithDerivatives(
      double x, double y, double z, int level) const;
  void getOccupancyGrid(nav_msgs::OccupancyGrid* message);

  // Sets the probablility for a "hit" (will be converted to logodds).
  void setHitProbability(double hit_probability);

  // Sets the probablility for a "miss" (will be converted to logodds).
  void setMissProbability(double miss_probability);

  // Sets the minimum occupancy for a cell. Occupancy values lower than the
  // minimum will be clamped.
  void setMinimumOccupancy(double minimum_occupancy);

  // Sets the maximum occupancy for a cell. Occupancy values greather than the
  // maximum will be clamped.
  void setMaximumOccupancy(double maximum_occupancy);

  // Resets (i.e. clears) the map state.
  void reset();

  const octomap::OcTree* getOcTree() const { return &octree_; };

 private:
  double getThreshold() const;
  void getMetricMin(double* x, double* y, double* z);
  void getMetricMax(double* x, double* y, double* z);
  void getPixelSize(int* x, int* y, int* z);

  octomap::OcTree octree_;
};

}  // namespace google_mapping

#endif  // GOOGLE_MAPPING_MAP_H_ 

